#include "stdafx.h"
#include "gp_Dir2d.h"
#include "gp_Trsf2d.h"
#include "gp_Pnt2d.h"
#include "gp.h"
#include "gp_Vec2d.h"
#include "gp_Ax2d.h"


bool gp_Vec2d::IsEqual
	(const gp_Vec2d& Other, 
	const double LinearTolerance,
	const double AngularTolerance) const
{
	const double theNorm = Magnitude();
	const double theOtherNorm = Other.Magnitude();
	double val = theNorm - theOtherNorm;
	if (val < 0.0) val = -val;
	// Check for equal lengths
	const bool isEqualLength = (val <= LinearTolerance);
	// Check for small vectors
	if (theNorm > LinearTolerance && theOtherNorm > LinearTolerance)
	{
		double Ang = Angle(Other);
		if (Ang < 0.0) Ang = -Ang;
		// Check for zero angle
		return isEqualLength && (Ang <= AngularTolerance);
	}
	return isEqualLength;
}    

double gp_Vec2d::Angle (const gp_Vec2d& Other) const
{
	//    Commentaires :
	//    Au dessus de 45 degres l'arccos donne la meilleur precision pour le
	//    calcul de l'angle. Sinon il vaut mieux utiliser l'arcsin.
	//    Les erreurs commises sont loin d'etre negligeables lorsque l'on est
	//    proche de zero ou de 90 degres.
	//    En 2D les valeurs angulaires sont comprises entre -PI et PI
	const double theNorm = Magnitude();
	const double theOtherNorm = Other.Magnitude();
	gp_VectorWithNullMagnitude_Raise_if(theNorm <= gp::Resolution() || theOtherNorm <= gp::Resolution(),"");
	const double D = theNorm * theOtherNorm;
	const double Cosinus = coord.Dot   (Other.coord) / D;
	const double Sinus = coord.Crossed (Other.coord) / D;
	if (Cosinus > -0.70710678118655 && Cosinus < 0.70710678118655)
	{
		if (Sinus > 0.0)  return  acos (Cosinus);
		else              return -acos (Cosinus); 
	}
	else
	{
		if (Cosinus > 0.0) return        asin (Sinus);
		else
		{ 
			if (Sinus > 0.0) return   M_PI - asin (Sinus);
			else             return - M_PI - asin (Sinus);
		}
	}  
}

void gp_Vec2d::Mirror (const gp_Ax2d& A1)
{
	const gp_XY& XY = A1.Direction().XY();
	double X = coord.X();
	double Y = coord.Y();
	double A = XY.X();
	double B = XY.Y();
	double M1 = 2.0 * A * B;
	coord.SetX(((2.0 * A * A) - 1.) * X + M1 * Y);
	coord.SetY(M1 * X + ((2. * B * B) - 1.0) * Y);
}

gp_Vec2d gp_Vec2d::Mirrored (const gp_Ax2d& A1) const
{
	gp_Vec2d Vres = *this;
	Vres.Mirror(A1);
	return Vres;                     
}

void gp_Vec2d::Transform (const gp_Trsf2d& T)
{
	if (T.Form() == gp_Identity || T.Form() == gp_Translation) { }
	else if (T.Form() == gp_PntMirror) coord.Reverse ();
	else if (T.Form() == gp_Scale)     coord.Multiply (T.ScaleFactor ());
	else                               coord.Multiply (T.VectorialPart ());
}

void gp_Vec2d::Mirror (const gp_Vec2d& V)
{
	const double D = V.coord.Modulus();
	if (D > gp::Resolution())
	{
		const gp_XY& XY = V.coord;
		double X = XY.X();
		double Y = XY.Y();
		double A = X / D;
		double B = Y / D;
		double M1 = 2.0 * A * B;
		coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y);
		coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y);
	}
}

gp_Vec2d gp_Vec2d::Mirrored (const gp_Vec2d& V) const
{
	gp_Vec2d Vres = *this;
	Vres.Mirror(V);
	return Vres;                     
}
 gp_Vec2d::gp_Vec2d()
{}

 gp_Vec2d::gp_Vec2d (const gp_Dir2d& V)
{ coord = V.XY(); }

 gp_Vec2d::gp_Vec2d (const gp_XY& Coord) : coord(Coord)
{}

 gp_Vec2d::gp_Vec2d (const double Xv,
			   const double Yv) : coord (Xv, Yv)
{ }

 gp_Vec2d::gp_Vec2d (const gp_Pnt2d& P1,
			   const gp_Pnt2d& P2)
{ coord = P2.XY().Subtracted (P1.XY()); }

 void gp_Vec2d::SetCoord (const int Index,
				const double Xi)
{ coord.SetCoord (Index, Xi); }

 void gp_Vec2d::SetCoord (const double Xv,
				const double Yv)
{ coord.SetCoord (Xv, Yv); }

 void gp_Vec2d::SetX (const double X)
{ coord.SetX (X); }

 void gp_Vec2d::SetY (const double Y)
{ coord.SetY (Y); }

 void gp_Vec2d::SetXY (const gp_XY& Coord)
{ coord = Coord; }

 double gp_Vec2d::Coord (const int Index) const
{ return coord.Coord(Index); }

 void gp_Vec2d::Coord(double& Xv,
			    double& Yv) const
{ coord.Coord(Xv, Yv); }

 double gp_Vec2d::X() const
{ return coord.X(); }
     
 double gp_Vec2d::Y() const
{ return coord.Y(); }

  const gp_XY& gp_Vec2d::XY () const
{ return coord; }

 bool gp_Vec2d::IsNormal
(const gp_Vec2d& Other, 
 const double AngularTolerance) const
{
  double Ang = Angle(Other);
  if (Ang < 0) Ang = - Ang;
  Ang = M_PI / 2.0 - Angle(Other);
  if (Ang < 0) Ang = - Ang;
  return  Ang <= AngularTolerance;
}    

 bool gp_Vec2d::IsOpposite
(const gp_Vec2d& Other,
 const double AngularTolerance) const
{
  double Ang = Angle(Other);
  if (Ang < 0) Ang = - Ang;
  return M_PI - Ang <= AngularTolerance;
}    

 bool gp_Vec2d::IsParallel
(const gp_Vec2d& Other,
 const double AngularTolerance) const
{
  double Ang = Angle(Other);
  if (Ang < 0) Ang = - Ang;
  return   Ang <= AngularTolerance || M_PI - Ang <= AngularTolerance;
}    

 double gp_Vec2d::Magnitude() const
{ return coord.Modulus(); }

 double gp_Vec2d::SquareMagnitude() const
{ return coord.SquareModulus(); }

 void gp_Vec2d::Add (const gp_Vec2d& Other)
{ coord.Add (Other.coord); }

 gp_Vec2d gp_Vec2d::Added (const gp_Vec2d& Other) const
{
  gp_Vec2d V = *this;
  V.coord.Add (Other.coord);
  return V;
}

 double gp_Vec2d::Crossed (const gp_Vec2d& Right) const
{ return coord.Crossed (Right.coord); }

 double gp_Vec2d::CrossMagnitude (const gp_Vec2d& Right) const
{ return coord.CrossMagnitude (Right.coord); }

 double gp_Vec2d::CrossSquareMagnitude
(const gp_Vec2d& Right) const
{ return coord.CrossSquareMagnitude (Right.coord); }

 void gp_Vec2d::Divide (const double Scalar)
{ coord.Divide (Scalar); }

 gp_Vec2d gp_Vec2d::Divided (const double Scalar) const
{
  gp_Vec2d V = *this;
  V.coord.Divide(Scalar);
  return V;
}

 double gp_Vec2d::Dot (const gp_Vec2d& Other) const
{ return coord.Dot (Other.coord); }

 void gp_Vec2d::Multiply (const double Scalar)
{ coord.Multiply (Scalar); }

 gp_Vec2d gp_Vec2d::Multiplied (const double Scalar) const
{
  gp_Vec2d V = *this;
  V.coord.Multiply(Scalar);
  return V;
}

 void gp_Vec2d::Normalize()
{ 
  double D = coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.Divide (D);
}

 gp_Vec2d gp_Vec2d::Normalized() const
{ 
  double D = coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  gp_Vec2d V = *this;
  V.coord.Divide (D);
  return V; 
}

 void gp_Vec2d::Reverse()
{ coord.Reverse(); }

 gp_Vec2d gp_Vec2d::Reversed() const
{
  gp_Vec2d V = *this;
  V.coord.Reverse();
  return V;
}

 void gp_Vec2d::Subtract (const gp_Vec2d& Right)
{ coord.Subtract (Right.coord); }

 gp_Vec2d gp_Vec2d::Subtracted (const gp_Vec2d& Right) const
{
  gp_Vec2d V = *this;
  V.coord.Subtract (Right.coord);
  return V;
}

 void gp_Vec2d::SetLinearForm (const double L, 
				     const gp_Vec2d& Left,
				     const double R,
				     const gp_Vec2d& Right)
{ coord.SetLinearForm (L, Left.coord, R, Right.coord); }

 void gp_Vec2d::SetLinearForm (const double L, 
				     const gp_Vec2d& Left,
				     const gp_Vec2d& Right)
{ coord.SetLinearForm (L, Left.coord, Right.coord); }

 void gp_Vec2d::SetLinearForm (const gp_Vec2d& Left,
				     const gp_Vec2d& Right)
{ coord.SetLinearForm (Left.coord,  Right.coord); }

 void gp_Vec2d::SetLinearForm (const double A1, 
				     const gp_Vec2d& V1,
				     const double A2, 
				     const gp_Vec2d& V2, 
				     const gp_Vec2d& V3)
{ coord.SetLinearForm (A1, V1.coord, A2, V2.coord, V3.coord); }

 void gp_Vec2d::Rotate (const double Ang)
{
  gp_Trsf2d T;
  T.SetRotation  (gp_Pnt2d (0.0, 0.0), Ang);
  coord.Multiply (T.VectorialPart ());
}

 gp_Vec2d gp_Vec2d::Rotated (const double Ang) const
{
  gp_Vec2d V = *this;
  V.Rotate (Ang);
  return V;
}

 void gp_Vec2d::Scale (const double S)
{ coord.Multiply (S); }

 gp_Vec2d gp_Vec2d::Scaled (const double S) const
{
  gp_Vec2d V = *this;
  V.coord.Multiply (S);
  return V;
}

 gp_Vec2d gp_Vec2d::Transformed (const gp_Trsf2d& T) const
{
  gp_Vec2d V = *this;
  V.Transform(T);
  return V;
} 

 gp_Vec2d operator* (const double Scalar,
			   const gp_Vec2d& V)
{ return V.Multiplied(Scalar); }

 gp_Vec2d gp_Vec2d::GetNormal() const
  {
  return gp_Vec2d(this->Y(), (-1)*this->X());
  }

